import zmq
import sys
import time
import smbus
sys.path.append('..')
from data_gathering.MPU6050 import mpu6050_action

# MPU6050 settings
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68 # This is the address value read via the i2cdetect command
# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
# END of MPU6050 settings


broker_ip = sys.argv[1]
agent_id = int(sys.argv[2])
# broker_ip = '192.168.137.79'
# agent_id = 2


def upload_data(publisher, sender_id, x_rotation, y_rotation, x_gyro_scaled):
    next_agent = 1  # center = 1
    publisher.send_string('%d:%f:%f:%d:%f' % (next_agent, x_rotation, y_rotation, sender_id, x_gyro_scaled))
    print("agent:" + str(agent_id))
    print("x rotation: " + str(x_rotation))
    print("y rotation: " + str(y_rotation))
    print("x_gyro_scaled: " + str(x_gyro_scaled))


def main(agent_id):
    context = zmq.Context()
    publisher = context.socket(zmq.PUB)
    publisher.connect("tcp://%s:5556" % broker_ip)

    subscriber = context.socket(zmq.SUB)
    subscriber.connect("tcp://%s:5555" % broker_ip)
    subscriber.setsockopt_string(zmq.SUBSCRIBE, str(agent_id))
    while True:
        # hand move, send data to agent1
        if agent_id == 2 or agent_id == 3:
            time.sleep(0.1)
            try:
                x_rotation, y_rotation,x_gyro_scaled = mpu6050_action(bus, address)
                upload_data(publisher, agent_id, x_rotation, y_rotation, x_gyro_scaled)
            except(IOError):
                print("io error")

        # wrong agent ID!
        else:
            print("wrong agent ID!")


if __name__ == "__main__":
    main(agent_id)
